
//LD293
const int moterLogic1a = 7;
const int moterLogic1b = 8;
const int moterLogic1e = 9;
const int moterLogic2a = 4;
const int moterLogic2b = 5;
const int moterLogic2e = 10;
const int speedPoti = A0;

const int maxStopDelay = 600;
unsigned long lastStop = 505;

const int minSpeed = 90;
int speed = 100;
int SollSpd = 0;
int msRampe = 2000;

//radEncoder
volatile int iencL = 0;
volatile int iencR = 0;
int getEncCount(){if(iencL<iencR){return iencL;}else{return iencR;}}

int Richtung;
const int dir_forward = 0;
const int dir_backward = 1;
const int dir_left = 2;
const int dir_right = 3;

void motorSetup(){
  pinMode(moterLogic1a, OUTPUT);
  pinMode(moterLogic1b, OUTPUT);
  pinMode(moterLogic1e, OUTPUT);
  pinMode(moterLogic2a, OUTPUT);
  pinMode(moterLogic2b, OUTPUT);
  pinMode(moterLogic2e, OUTPUT);
}

void UpdateSpeed(){
  SollSpd = analogRead(speedPoti);
  SollSpd = map(SollSpd, 0, 1023, minSpeed, 255);

  unsigned long TimeDiff;
  TimeDiff = millis() - lastStop;

  float Faktor;
  Faktor = TimeDiff/float(msRampe);
  if(Faktor > 1.000){
    Faktor = 1.0;
  }

  speed = Faktor * SollSpd;
  if(speed < minSpeed){
    speed = minSpeed;
  }
  //  Serial.print("speed: "); Serial.println(speed);

  setSpeed(speed);
}

void resetSpeed(){
  setSpeed(0);
  lastStop=millis();
  UpdateSpeed();
}

void forward(){
  logMsg('F');

  if (Richtung != dir_forward){
    stopp();
  }
  Richtung = dir_forward;
  setMotorLogic(HIGH, LOW, HIGH, LOW);
}

void backward(){
  logMsg('B');
  if (Richtung != dir_backward){
    stopp();
  }
  Richtung = dir_backward;
  setMotorLogic(LOW, HIGH, LOW, HIGH);
}

void right(){
  logMsg('R');
  if (Richtung != dir_right){
    stopp();
  }
  Richtung = dir_right;
  setMotorLogic(HIGH, LOW, LOW, HIGH);
}

void left(){
  logMsg('L');
  if (Richtung != dir_left){
    stopp();
  }
  Richtung = dir_left;
  setMotorLogic(LOW, HIGH, HIGH, LOW);
}

void shortback(){
  logMsg("ShortBack");
  backward();
  driveFor(2);
}

void stopp() { 
  //logMsg("Stop");
  setSpeed(0);

  int diff=millis()-lastStop;
  if(diff>30){
    float Faktor;
    Faktor = float(speed)/float(SollSpd);
    if(Faktor > 1.000){
      Faktor = 1.0;
    }

    unsigned long time = 0;
    time = maxStopDelay * Faktor;
    if(time < 100){
      time = 100;
    }
    Serial.print("Stopp: "); Serial.println(time);
    
    delay(time);
  }
  lastStop=millis();
}

unsigned long startTime;

void driveFor(byte aSteps){
  //logMsg("driveFor");
  NullEnc();
  
  startTime = millis(); 
  int SENS = sens();
  if(aSteps > 1){  //beide encoder müssen sich einmal wechseln
    while(((iencR < aSteps) || (iencL < aSteps)) && (millis() - startTime < 2500) || (millis() - startTime < 120))  
    {UpdateSpeed();} 
  }
  
  else{ //nur einer muss die Anzahl Wechsel gemacht haben
    while(((iencR < aSteps) && (iencL < aSteps)) && (millis() - startTime < 2500) && (SENS == sens()))  
    {UpdateSpeed();}  
  }
  NullEnc();
}

void NullEnc(){
  iencR = 0;
  iencL = 0;
}
  
void setMotorLogic(byte m11, byte m12, byte m21, byte m22){
  set(moterLogic1a,m11);
  set(moterLogic1b,m12);
  set(moterLogic2a,m21);
  set(moterLogic2b,m22);
}

void setSpeed(int aSpeed) {
  analogWrite(moterLogic1e, aSpeed);
  analogWrite(moterLogic2e, aSpeed);
}



